#include "physics.h"

Physics::Physics()
{
  ftdi = new Imu();
  
  T1[0] = 5;
  T1[1] = 0;
  T1[2] = 0;
  T2[0] = -5*sin(30.0/180*PI);
  T2[1] = -5*cos(30.0/180*PI);
  T2[2] = 0;
  T3[0] = -5*sin(30.0/180*PI);
  T3[1] = 5*cos(30.0/180*PI);
  T3[2] = 0;
  T1p[2] = 0;
  T2p[2] = 0;
  T3p[2] = 0;
  
}

void Physics::run()
{	
  update();
  calcThrusterPositions();
}

double Physics::getChannel(int index)
{
  return ftdi->getIMUData(index);
}

void Physics::applyRotation(double prev[],double prime[])
{
  double a1 = ftdi->getIMUData(2); //roll (y rotation)
  double a2 = ftdi->getIMUData(1); //pitch (x rotation)
  double a3 = ftdi->getIMUData(0); //yaw (z rotation)
  
  if(a1 < 0)
    a1 += 360;
  if(a2 < 0)
    a2 += 360;
  if(a3 < 0)
    a3 += 360;
    
  a1 = a1/180*PI;
  a2 = a2/180*PI;
  a3 = a3/180*PI; 

  double Rd[3][3] = {{cos(a3),sin(a3),0},{-sin(a3),cos(a3),0},{0,0,1}};
  double Rb[3][3] = {{1,0,0},{0,cos(a1),sin(a1)},{0,-sin(a1),cos(a1)}};
  double Rc[3][3] = {{cos(a2),0,-sin(a2)},{0,1,0},{sin(a2),0,cos(a2)}};
  double Ra[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
  double Rt[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
  
  prime[0] = 0;
  prime[1] = 0;
  prime[2] = 0;
  
  for(int i = 0; i < 3; i++)
  {
    for(int j = 0; j < 3; j++)
    {
      for(int k = 0; k < 3; k++)
      {
        Rt[i][j] += Rc[i][k]*Rb[k][j];
      }
    }
  }
  
  for(int i = 0; i < 3; i++)
  {
    for(int j = 0; j < 3; j++)
    {
      for(int k = 0; k < 3; k++)
      {
        Ra[i][j] += Rd[i][k]*Rt[k][j];
      }
    }
  }
  
  for(int i = 0; i < 3; i++)
  {
    for(int k = 0; k < 3; k++)
    {
      prime[i] += Ra[i][k]*prev[k];
    }
  }
}

double Physics::getThrusterPositions(int index)
{
	if(index == 0)
    return T1p[2];
  else if(index == 1)
    return T2p[2];
  else
    return T3p[2];
}

void Physics::calcThrusterPositions()
{
  applyRotation(T1,T1p);
  applyRotation(T2,T2p);
  applyRotation(T3,T3p);
}

void Physics::update()
{
  ftdi->readChannels();
}
